/*
 * Entech2013DriveSubsystem.cpp
 *
 *  Created on: Feb 3, 2013
 *      Author: N500432
 */

#include "Entech2013DriveSubsystem.h"
#include "EntechPorts.h"

#include <math.h>

// Default constructor
Entech2013DriveSubsystem::Entech2013DriveSubsystem() :
	EntechSubsystem("DriveSubsystem")
	, m_driveType(kArcadeDrive)
	, m_shifterState(kLowGear)
	, m_state(kJoystick)
	, m_motorSpeedDelta(0.0)
{
    m_leftMotor  = new Talon(c_DriveLeftMotorPort);
    m_rightMotor = new Talon(c_DriveRightMotorPort);

    // Last argument is a "reverser" for the encoder
    m_leftEnc  = new Encoder(c_DriveLeftEncoderAport,c_DriveLeftEncoderBport,false);
    m_rightEnc = new Encoder(c_DriveRightEncoderAport,c_DriveRightEncoderBport,false);
    m_leftEnc->SetDistancePerPulse(c_distancePerEncoderPulse);
    m_rightEnc->SetDistancePerPulse(c_distancePerEncoderPulse);
    
    m_highShifterSolenoid = new Solenoid(c_ShifterHighPort);
 
    m_timer = new Timer();
    
    m_leftStick  = Joystick::GetStickForPort(c_DriverLeftJoystick);
    m_rightStick = Joystick::GetStickForPort(c_DriverRightJoystick);
}

// Default destructor
Entech2013DriveSubsystem::~Entech2013DriveSubsystem()
{
}

void Entech2013DriveSubsystem::DoRobotInit(void)
{
    m_leftEnc->Start();
    m_rightEnc->Start();
}

void Entech2013DriveSubsystem::DoDisabledInit(void)
{
	m_state = kStopped;
}

void Entech2013DriveSubsystem::DoAutonomousInit(void)
{
	m_state = kStopped;
    m_shifterState = kLowGear;
	m_highShifterSolenoid->Set(false);
}

void Entech2013DriveSubsystem::DoTeleopInit(void)
{
	m_state = kJoystick;
}

void Entech2013DriveSubsystem::DoDisabled(void)
{
    Stop();
}

void Entech2013DriveSubsystem::DoAutonomous(void)
{
	switch (m_state) {
	case kJoystick:
	case kStopped:
	case kOff:
		Stop();
		break;
	case kTurning:
	case kStraight:
	case kTimed:
		AutomaticDrive();
		break;
	}
	DoShifter();
}

void Entech2013DriveSubsystem::DoTeleop(void)
{
	// Do Drive operations
	switch (m_state) {
	case kJoystick:
		JoystickDrive(m_leftStick, m_rightStick);
		break;
	case kStopped:
	case kOff:
		Stop();
		break;
	case kTurning:
	case kStraight:
	case kTimed:
		AutomaticDrive();
		break;
	}
	DoShifter();
	
	// Print State Information
    DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
    if (kArcadeDrive == m_driveType) {
        dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Drive: Arcade ");
    } else if (kTankDrive == m_driveType) {
        dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Drive: Tank   ");
    }
    if (kLowGear == m_shifterState) {
        dsLCD->Printf(DriverStationLCD::kUser_Line2, 16, " |  Low Gear ");
    } else if (kHighGear == m_shifterState) {
        dsLCD->Printf(DriverStationLCD::kUser_Line2, 16, " |  High Gear");
    }
    dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Encoders: Left %d  - Right %d",
    		m_leftEnc->Get(), m_rightEnc->Get());   
}

void Entech2013DriveSubsystem::JoystickDrive(Joystick *js_left, Joystick *js_right)
{
    double jx, jy;
    double lmotor, rmotor;
    
    // Deadzone constants
    const double c_xdead = 0.05;
    const double c_ydead = 0.05;

    switch (m_driveType) {
    case kArcadeDrive:
        jx = ApplyDeadzone(js_left->GetX(), c_xdead);
        jy = ApplyDeadzone(js_left->GetY(), c_ydead);

        // Logic stolen from WPILib
        if (jy > 0.0) {
            if (jx > 0.0) {
                lmotor = jy - jx;
                rmotor = max(jy, jx);
            } else {
                lmotor = max(jy, -jx);
                rmotor = jy + jx;
            }
        } else {
            if (jx > 0.0) {
                lmotor = -max(-jy, jx);
                rmotor = jy + jx;
            } else {
                lmotor = jy - jx;
                rmotor = -max(-jy, -jx);
            }
        }
        SetDriveMotors(lmotor, rmotor);
        break;
    case kTankDrive:
        lmotor = ApplyDeadzone(js_left->GetY(), c_ydead);
        rmotor = ApplyDeadzone(js_right->GetY(), c_ydead);
        SetDriveMotors(lmotor, rmotor);
        break;
    }
}

// Manage motor inversions and set motor speeds
void Entech2013DriveSubsystem::SetDriveMotors(double lmotor, double rmotor)
{
	lmotor = min(1.0,max(-1.0,lmotor));
	rmotor = min(1.0,max(-1.0,rmotor));
    m_leftMotor->Set(c_left_inverter*lmotor);
    m_rightMotor->Set(c_right_inverter*rmotor);
}

double Entech2013DriveSubsystem::ApplyDeadzone(double input, double deadzone)
{
    double value;

    // verify assumptions
    //assert(deadzone >= 0.0);
    //assert(fabs(input) <= 1.0);
    
    if (fabs(input) < deadzone) {
        value = 0.0;
    } else if (input < 0.0) {
        value = (input + deadzone) / (1.0 - deadzone);
    } else {
        value = (input - deadzone) / (1.0 - deadzone);
    }
    return value;
}

// Drive the robot to the previously requested Turn/Distance
void Entech2013DriveSubsystem::AutomaticDrive(void)
{
	bool left_done, right_done;
	double lmotor, rmotor;
	double left_pos, right_pos;
	
	left_pos = m_leftEnc->GetDistance();
	right_pos = m_rightEnc->GetDistance();
	
	left_done = false;
	right_done = false;
	
	if ( kTimed == m_state ) {
		if (m_timer->Get() > m_timeout) {
			m_timer->Stop();
			Stop();
		} else {
			SetDriveMotors(m_reqSpeed, m_reqSpeed);
		}
		return;
	}
	if (fabs(left_pos) > fabs(m_leftEncTarget)) {
		left_done = true;
	}
	if (fabs(right_pos) > fabs(m_rightEncTarget)) {
		right_done = true;
	}
	if (left_done && right_done) {
		Stop();
	} else {
		if ( fabs(left_pos - m_leftEncLastPos) > fabs(right_pos - m_rightEncLastPos) ) {
			m_motorSpeedDelta += 0.005;
		} else if ( fabs(left_pos - m_leftEncLastPos) < fabs(right_pos - m_rightEncLastPos) ) {
			m_motorSpeedDelta -= 0.005;
		}
		lmotor = (m_leftEncTarget > 0) ? (m_reqSpeed - m_motorSpeedDelta) : -(m_reqSpeed - m_motorSpeedDelta);
		rmotor = (m_rightEncTarget > 0) ? (m_reqSpeed + m_motorSpeedDelta): -(m_reqSpeed + m_motorSpeedDelta);
		SetDriveMotors(lmotor, rmotor);
	}
	m_leftEncLastPos = left_pos;
	m_rightEncLastPos = right_pos;
}

void Entech2013DriveSubsystem::Turn(double angle)
{
    // Use Encoders and turn by specified angle
	m_leftEnc->Reset();
	m_rightEnc->Reset();
	m_state = kTurning;
	m_leftEncTarget  = -angle * c_encoderDistancePerDeg;
	m_rightEncTarget =  angle * c_encoderDistancePerDeg;
	m_reqSpeed = 0.25;
}

void Entech2013DriveSubsystem::StraightDistance(double distance, double speed)
{
    // Use Encoders and drive straight a specified distance
	m_leftEnc->Reset();
	m_rightEnc->Reset();
	m_state = kStraight;
	m_leftEncTarget  = distance;
	m_rightEncTarget = distance;
	m_reqSpeed = speed;
}

void Entech2013DriveSubsystem::StraightTime(double time, double speed)
{
    // Use Encoders and drive straight a specified distance
	m_leftEnc->Reset();
	m_rightEnc->Reset();
	m_timer->Stop();
	m_timer->Reset();
	m_timer->Start();
	m_state = kTimed;
	m_reqSpeed = speed;
}

void Entech2013DriveSubsystem::Stop(void)
{
	m_state = kStopped;
	m_shifterState = kLowGear;
	m_highShifterSolenoid->Set(false);
    SetDriveMotors(0.0,0.0);
}

void Entech2013DriveSubsystem::Off(void)
{
	m_state = kOff;
	m_shifterState = kLowGear;
	m_highShifterSolenoid->Set(false);
    SetDriveMotors(0.0,0.0);
}

bool Entech2013DriveSubsystem::CommandDone(void)
{
	if ((kTurning == m_state) || (kStraight == m_state) || (kTimed == m_state)) {
		return false;
	}
	return true;
}

void Entech2013DriveSubsystem::ForceOn(void)
{
	m_state = kJoystick;
}

void Entech2013DriveSubsystem::ToggleDriveType(void)
{
	if (m_driveType == kArcadeDrive) {
		m_driveType = kTankDrive;
	} else {
		m_driveType = kArcadeDrive;
	}
}

void Entech2013DriveSubsystem::ShiftHigh(void)
{
    m_shifterState = kHighGear;
}

void Entech2013DriveSubsystem::ShiftLow(void)
{
    m_shifterState = kLowGear;
}

void Entech2013DriveSubsystem::DoShifter(void)
{
	if ( kHighGear == m_shifterState ) {
		m_highShifterSolenoid->Set(true);
	} else {
		m_highShifterSolenoid->Set(false);
	}
}
